/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2020 Shivang Patel
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Shivang Patel
*
* Reference tutorial:
* https://navigation.ros.org/tutorials/docs/writing_new_nav2planner_plugin.html
*********************************************************************/

#include <cmath>
#include <string>
#include <memory>
#include "nav2_util/node_utils.hpp"

#include "full_coverage_path_planner_ros2/full_coverage_path_planner.hpp"

namespace full_coverage_path_planner {

FullCoveragePath::FullCoveragePath() : tf_(nullptr), costmap_(nullptr) {
}

FullCoveragePath::~FullCoveragePath() {
    RCLCPP_INFO(
            node_->get_logger(), "Destroying plugin %s of type FullCoveragePath",
            name_.c_str());
}

void FullCoveragePath::configure(
        const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
        std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
        std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
    node_ = parent.lock();
    name_ = name;
    tf_ = tf;
    costmap_ = costmap_ros->getCostmap();
    global_frame_ = costmap_ros->getGlobalFrameID();
    cpp_grid_client_ = node_->create_client<nav2_msgs::srv::GetMapNav2>("/map_server/map");

    // Parameter initialization

    nav2_util::declare_parameter_if_not_declared(node_, name_ + ".robot_radius", rclcpp::ParameterValue(0.15));
    node_->get_parameter(name_ + ".robot_radius", robotRadius);
    nav2_util::declare_parameter_if_not_declared(node_, name_ + ".tool_radius", rclcpp::ParameterValue(0.15));
    node_->get_parameter(name_ + ".tool_radius", toolRadius);
}

void FullCoveragePath::cleanup() {
    RCLCPP_INFO(
            node_->get_logger(), "CleaningUp plugin %s of type FullCoveragePath",
            name_.c_str());
}

void FullCoveragePath::activate() {
    RCLCPP_INFO(
            node_->get_logger(), "Activating plugin %s of type FullCoveragePath",
            name_.c_str());
}

void FullCoveragePath::deactivate() {
    RCLCPP_INFO(
            node_->get_logger(), "Deactivating plugin %s of type FullCoveragePath",
            name_.c_str());
}


nav_msgs::msg::Path FullCoveragePath::createPlan(
        const geometry_msgs::msg::PoseStamped &start,
        const geometry_msgs::msg::PoseStamped &goal) {
    nav_msgs::msg::Path global_path;
    Point_t startPoint;

    // Checking if the goal and start state is in the global frame
    if (start.header.frame_id != global_frame_) {
        RCLCPP_ERROR(
                node_->get_logger(), "Planner will only except start position from %s frame",
                global_frame_.c_str());
        return global_path;
    }

    if (goal.header.frame_id != global_frame_) {
        RCLCPP_INFO(
                node_->get_logger(), "Planner will only except goal position from %s frame",
                global_frame_.c_str());
        return global_path;
    }

    global_path.poses.clear();
    global_path.header.stamp = node_->now();
    global_path.header.frame_id = global_frame_;

    /********************** Get grid from server **********************/
    std::vector<std::vector<bool> > grid;
    auto grid_req_srv = std::make_shared<nav2_msgs::srv::GetMapNav2::Request>();
    std::cout << "Requesting grid!!\n\n";
    if (!cpp_grid_client_->wait_for_service(std::chrono::seconds(5))) {
        std::cout << "Could not retrieve grid from map_server\n";
        return global_path;
    }
    std::cout << "Requesting grid  success!!\n\n";

    auto result_future = cpp_grid_client_->async_send_request(grid_req_srv);

    if (!parseGrid(result_future.get()->map, grid, robotRadius * 2, toolRadius * 2, start, startPoint)) {
        std::cout << "Could not parse retrieved grid\n";
        return global_path;
    }
    std::list<Point_t> goalPoints = spiral_stc(grid,
                                               startPoint,
                                               spiral_cpp_metrics_.multiple_pass_counter,
                                               spiral_cpp_metrics_.visited_counter);
    std::cout << "naive cpp completed!\n";
    std::cout << "Converting path to plan\n";
    std::vector<geometry_msgs::msg::PoseStamped> plan;
    parsePointlist2Plan(start, goalPoints, plan);
    // Print some metrics:
    spiral_cpp_metrics_.accessible_counter = spiral_cpp_metrics_.visited_counter
                                             - spiral_cpp_metrics_.multiple_pass_counter;
    spiral_cpp_metrics_.total_area_covered =
            (4.0 * robotRadius * toolRadius) * spiral_cpp_metrics_.accessible_counter;
    std::cout << "Total visited: " << spiral_cpp_metrics_.visited_counter << "\n";
    std::cout << "Total re-visited: " << spiral_cpp_metrics_.multiple_pass_counter << "\n";
    std::cout << "Total accessible cells: " << spiral_cpp_metrics_.accessible_counter << "\n";
    std::cout << "Total accessible area: " << spiral_cpp_metrics_.total_area_covered << "\n";

    if (!plan.empty()) {
        global_path.header.frame_id = plan[0].header.frame_id;
        global_path.header.stamp = plan[0].header.stamp;
    }
    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    bool removed_some_head = false;

    for (unsigned int i = 0; i < plan.size(); i++) {
        if (!removed_some_head && i > 0) {
            if (std::hypot(plan[0].pose.position.x - plan[i].pose.position.x,
                           plan[0].pose.position.y - plan[i].pose.position.y) <= 0.15) {
                continue;
            } else {
                removed_some_head = true;
            }
        }
        global_path.poses.push_back(plan[i]);
    }
    //global_path.poses.push_back(goal);
    return global_path;
}


std::list<gridNode_t>
FullCoveragePath::spiral(std::vector<std::vector<bool> > const &grid,
                         std::list<gridNode_t> &init,
                         std::vector<std::vector<bool> > &visited) {
    int dx, dy, dx_prev, x2, y2, i, nRows = grid.size(), nCols = grid[0].size();
    // Spiral filling of the open space
    // Copy incoming list to 'end'
    std::list<gridNode_t> pathNodes(init);
    // Create iterator for gridNode_t list and let it point to the last element of end
    std::list<gridNode_t>::iterator it = --(pathNodes.end());
    if (pathNodes.size() > 1)  // if list is length 1, keep iterator at end
        it--;                    // Let iterator point to second to last element

    gridNode_t prev = *(it);
    bool done = false;
    while (!done) {
        if (it != pathNodes.begin()) {
            // turn ccw
            dx = pathNodes.back().pos.x - prev.pos.x;
            dy = pathNodes.back().pos.y - prev.pos.y;
            dx_prev = dx;
            dx = -dy;
            dy = dx_prev;
        } else {
            // Initialize spiral direction towards y-axis
            dx = 0;
            dy = 1;
        }
        done = true;

        for (int i = 0; i < 4; ++i) {
            x2 = pathNodes.back().pos.x + dx;
            y2 = pathNodes.back().pos.y + dy;
            if (x2 >= 0 && x2 < nCols && y2 >= 0 && y2 < nRows) {
                if (grid[y2][x2] == eNodeOpen && visited[y2][x2] == eNodeOpen) {
                    Point_t new_point = {x2, y2};
                    gridNode_t new_node =
                            {
                                    new_point,  // Point: x,y
                                    0,          // Cost
                                    0,          // Heuristic
                            };
                    prev = pathNodes.back();
                    pathNodes.push_back(new_node);
                    it = --(pathNodes.end());
                    visited[y2][x2] = eNodeVisited;  // Close node
                    done = false;
                    break;
                }
            }
            // try next direction cw
            dx_prev = dx;
            dx = dy;
            dy = -dx_prev;
        }
    }
    return pathNodes;
}

std::list<Point_t> FullCoveragePath::spiral_stc(
        std::vector<std::vector<bool> > const &grid,
        Point_t &init,
        int &multiple_pass_counter,
        int &visited_counter) {
    int x, y, nRows = grid.size(), nCols = grid[0].size();
    // Initial node is initially set as visited so it does not count
    multiple_pass_counter = 0;
    visited_counter = 0;

    std::vector<std::vector<bool> > visited;
    visited = grid;  // Copy grid matrix
    x = init.x;
    y = init.y;

    Point_t new_point = {x, y};
    gridNode_t new_node =
            {
                    new_point,  // Point: x,y
                    0,          // Cost
                    0,          // Heuristic
            };
    std::list<gridNode_t> pathNodes;
    std::list<Point_t> fullPath;
    pathNodes.push_back(new_node);
    visited[y][x] = eNodeVisited;

#ifdef DEBUG_PLOT
    ROS_INFO("Grid before walking is: ");
    printGrid(grid, visited, fullPath);
#endif

    pathNodes = spiral(grid, pathNodes, visited);                // First spiral fill
    std::list<Point_t> goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints
    // Add points to full path
    std::list<gridNode_t>::iterator it;
    for (it = pathNodes.begin(); it != pathNodes.end(); ++it) {
        Point_t newPoint = {it->pos.x, it->pos.y};
        visited_counter++;
        fullPath.push_back(newPoint);
    }
    // Remove all elements from pathNodes list except last element
    pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));

#ifdef DEBUG_PLOT
    ROS_INFO("Current grid after first spiral is");
    printGrid(grid, visited, fullPath);
    ROS_INFO("There are %d goals remaining", goals.size());
#endif
    while (goals.size() != 0) {
        // Remove all elements from pathNodes list except last element.
        // The last point is the starting point for a new search and A* extends the path from there on
        pathNodes.erase(pathNodes.begin(), --(pathNodes.end()));
        visited_counter--;  // First point is already counted as visited
        // Plan to closest open Node using A*
        // `goals` is essentially the map, so we use `goals` to determine the distance from the end of a potential path
        //    to the nearest free space
        bool resign = a_star_to_open_space(grid, pathNodes.back(), 1, visited, goals, pathNodes);
        if (resign) {
#ifdef DEBUG_PLOT
            ROS_INFO("A_star_to_open_space is resigning", goals.size());
#endif
            break;
        }

        // Update visited grid
        for (it = pathNodes.begin(); it != pathNodes.end(); ++it) {
            if (visited[it->pos.y][it->pos.x]) {
                multiple_pass_counter++;
            }
            visited[it->pos.y][it->pos.x] = eNodeVisited;
        }
        if (pathNodes.size() > 0) {
            multiple_pass_counter--;  // First point is already counted as visited
        }

#ifdef DEBUG_PLOT
        ROS_INFO("Grid with path marked as visited is:");
        gridNode_t SpiralStart = pathNodes.back();
        printGrid(grid, visited, pathNodes, pathNodes.front(), pathNodes.back());
#endif

        // Spiral fill from current position
        pathNodes = spiral(grid, pathNodes, visited);

#ifdef DEBUG_PLOT
        ROS_INFO("Visited grid updated after spiral:");
        printGrid(grid, visited, pathNodes, SpiralStart, pathNodes.back());
#endif

        goals = map_2_goals(visited, eNodeOpen);  // Retrieve remaining goalpoints

        for (it = pathNodes.begin(); it != pathNodes.end(); ++it) {
            Point_t newPoint = {it->pos.x, it->pos.y};
            visited_counter++;
            fullPath.push_back(newPoint);
        }
    }

    return fullPath;
}

bool FullCoveragePath::parseGrid(nav_msgs::msg::OccupancyGrid const &cpp_grid_,
                                 std::vector<std::vector<bool> > &grid,
                                 float robotRadius,
                                 float toolRadius,
                                 geometry_msgs::msg::PoseStamped const &realStart,
                                 Point_t &scaledStart) {
    int ix, iy, nodeRow, nodeColl;
    uint32_t nodeSize = dmax(floor(toolRadius / cpp_grid_.info.resolution), 1);  // Size of node in pixels/units
    uint32_t robotNodeSize = dmax(floor(robotRadius / cpp_grid_.info.resolution),
                                  1);  // RobotRadius in pixels/units
    uint32_t nRows = cpp_grid_.info.height, nCols = cpp_grid_.info.width;
    std::cout << "nRows: " << nRows << " nCols: " << nCols << " nodeSize: " << nodeSize << "\n";

    if (nRows == 0 || nCols == 0) {
        return false;
    }

    // Save map origin and scaling
    tile_size_ = nodeSize * cpp_grid_.info.resolution;  // Size of a tile in meters
    grid_origin_.x = cpp_grid_.info.origin.position.x;  // x-origin in meters
    grid_origin_.y = cpp_grid_.info.origin.position.y;  // y-origin in meters

    // Scale starting point
    scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
                                                    floor(cpp_grid_.info.width / tile_size_)));
    scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
                                                    floor(cpp_grid_.info.height / tile_size_)));

    // Scale grid
    for (iy = 0; iy < nRows; iy = iy + nodeSize) {
        std::vector<bool> gridRow;
        for (ix = 0; ix < nCols; ix = ix + nodeSize) {
            bool nodeOccupied = false;
            for (nodeRow = 0;
                 (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow) {
                for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl) {
                    int index_grid = dmax((iy + nodeRow - ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0))
                                          * nCols + (ix + nodeColl -
                                                     ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0)), 0);
                    if (cpp_grid_.data[index_grid] > 65) {
                        nodeOccupied = true;
                        break;
                    }
                }
            }
            gridRow.push_back(nodeOccupied);
        }
        grid.push_back(gridRow);
    }
    return true;
}

void FullCoveragePath::parsePointlist2Plan(const geometry_msgs::msg::PoseStamped &start,
                                           std::list<Point_t> const &goalpoints,
                                           std::vector<geometry_msgs::msg::PoseStamped> &plan) {
    geometry_msgs::msg::PoseStamped new_goal;
    tf2::Quaternion q_ium;
    std::list<Point_t>::const_iterator it, it_next, it_prev;
    int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_prev = 0, move_dir_next = 0;
    bool do_publish = false;
    float orientation = eDirNone;
    std::cout << "Received goalpoints with length: " << goalpoints.size() << "\n";
    if (goalpoints.size() > 1) {
        for (it = goalpoints.begin(); it != goalpoints.end(); ++it) {
            it_next = it;
            it_next++;
            it_prev = it;
            it_prev--;

            // Check for the direction of movement
            if (it == goalpoints.begin()) {
                dx_now = it_next->x - it->x;
                dy_now = it_next->y - it->y;
            } else {
                dx_now = it->x - it_prev->x;
                dy_now = it->y - it_prev->y;
                dx_next = it_next->x - it->x;
                dy_next = it_next->y - it->y;
            }

            // Calculate direction enum: dx + dy*2 will give a unique number for each of the four possible directions because
            // of their signs:
            //  1 +  0*2 =  1
            //  0 +  1*2 =  2
            // -1 +  0*2 = -1
            //  0 + -1*2 = -2
            move_dir_now = dx_now + dy_now * 2;
            move_dir_next = dx_next + dy_next * 2;

            // Check if this points needs to be published (i.e. a change of direction or first or last point in list)
            do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() ||
                         (it != goalpoints.end() && it == --goalpoints.end());
            move_dir_prev = move_dir_now;

            // Add to vector if required
            if (do_publish) {
                new_goal.header.frame_id = "map";
                new_goal.pose.position.x = (it->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5;
                new_goal.pose.position.y = (it->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5;
                // Calculate desired orientation to be in line with movement direction
                switch (move_dir_now) {
                    case eDirNone:
                        // Keep orientation
                        break;
                    case eDirRight:
                        orientation = 0;
                        break;
                    case eDirUp:
                        orientation = M_PI / 2;
                        break;
                    case eDirLeft:
                        orientation = M_PI;
                        break;
                    case eDirDown:
                        orientation = M_PI * 1.5;
                        break;
                }

                q_ium.setRPY(0, 0, orientation);
                new_goal.pose.orientation.w = q_ium.getW();
                new_goal.pose.orientation.x = q_ium.getX();
                new_goal.pose.orientation.y = q_ium.getY();
                new_goal.pose.orientation.z = q_ium.getZ();
                if (it != goalpoints.begin()) {
                    previous_goal_.pose.orientation = new_goal.pose.orientation;
                    // republish previous goal but with new orientation to indicate change of direction
                    // useful when the plan is strictly followed with base_link
                    plan.push_back(previous_goal_);
                }
                std::cout << "Voila new point: x=" << new_goal.pose.position.x << ", y=" << new_goal.pose.position.y
                          << ", o=" << new_goal.pose.orientation.x
                          << " " << new_goal.pose.orientation.y << " " << new_goal.pose.orientation.z << " "
                          << new_goal.pose.orientation.w << "\n";
                plan.push_back(new_goal);
                previous_goal_ = new_goal;
            }
        }
    } else {
        new_goal.header.frame_id = "map";
        new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5;
        new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5;
        q_ium.setRPY(0, 0, 0);
        new_goal.pose.orientation.w = q_ium.getW();
        new_goal.pose.orientation.x = q_ium.getX();
        new_goal.pose.orientation.y = q_ium.getY();
        new_goal.pose.orientation.z = q_ium.getZ();
        plan.push_back(new_goal);
    }
    /* Add poses from current position to start of plan */

    // Compute angle between current pose and first plan point
    double dy = plan.begin()->pose.position.y - start.pose.position.y;
    double dx = plan.begin()->pose.position.x - start.pose.position.x;
    // Arbitrary choice of 100.0*FLT_EPSILON to determine minimum angle precision of 1%
    if (!(fabs(dy) < 100.0 * FLT_EPSILON && fabs(dx) < 100.0 * FLT_EPSILON)) {
        // Add extra translation waypoint
        double yaw = std::atan2(dy, dx);
        geometry_msgs::msg::Quaternion quat_temp;
        q_ium.setRPY(0, 0, yaw);
        quat_temp.set__x(q_ium.getX());
        quat_temp.set__y(q_ium.getY());
        quat_temp.set__z(q_ium.getZ());
        quat_temp.set__w(q_ium.getW());

        geometry_msgs::msg::PoseStamped extra_pose;
        extra_pose = *plan.begin();
        extra_pose.pose.orientation = quat_temp;
        plan.insert(plan.begin(), extra_pose);
        extra_pose = start;
        extra_pose.pose.orientation = quat_temp;
        plan.insert(plan.begin(), extra_pose);
    }

    // Insert current pose
    plan.insert(plan.begin(), start);
    std::cout << "Plan ready containing " << plan.size() << " goals! \n";
}


}  // namespace full_coverage_path_planner

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(full_coverage_path_planner::FullCoveragePath, nav2_core::GlobalPlanner)
